#include <iostream>
#include <opencv4/opencv2/opencv.hpp>
#include <fcntl.h>
#include <unistd.h>
#include <sys/ioctl.h>
#include <linux/videodev2.h>
#include <sys/mman.h>
#include <string>
using namespace std;
using namespace cv;

int main() {
    // 打开摄像头设备
    std::string device_str;
    std::cout << "Enter video device path (e.g., /dev/video0): ";
    std::cin >> device_str;
    const char *device = device_str.c_str();
    int fd = open(device, O_RDWR);
    if (fd == -1) {
        cerr << "Error opening device: " << device << endl;
        return 1;
    }

    // 查询设备能力
    v4l2_capability cap;
    if (ioctl(fd, VIDIOC_QUERYCAP, &cap) == -1) {
        cerr << "Error querying device capabilities" << endl;
        close(fd);
        return 1;
    }
    // 设置帧率
    v4l2_streamparm streamparm;
    streamparm.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
    if (ioctl(fd, VIDIOC_G_PARM, &streamparm) == -1) {
        std::cerr << "Error getting stream parameters" << std::endl;
        close(fd);
        return 1;
    }

    streamparm.parm.capture.timeperframe.numerator = 1;
    streamparm.parm.capture.timeperframe.denominator = 30; // 设置帧率为 30 fps

    if (ioctl(fd, VIDIOC_S_PARM, &streamparm) == -1) {
        std::cerr << "Error setting frame rate" << std::endl;
        close(fd);
        return 1;
    }

    std::cout << "Frame rate set to 30 fps" << std::endl;
    // 设置视频格式
    v4l2_format fmt;
    memset(&fmt, 0, sizeof(fmt));
    fmt.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
    fmt.fmt.pix.width = 640; // 设置宽度
    fmt.fmt.pix.height = 360; // 设置高度
    fmt.fmt.pix.pixelformat = V4L2_PIX_FMT_YUYV; // 设置像素格式
    fmt.fmt.pix.field = V4L2_FIELD_NONE;
    if (ioctl(fd, VIDIOC_S_FMT, &fmt) == -1) {
        cerr << "Error setting pixel format" << endl;
        close(fd);
        return 1;
    }


    // 分配内存并将缓冲区映射到用户空间
    v4l2_requestbuffers req;
    memset(&req, 0, sizeof(req));
    req.count = 1; // 设置缓冲区数量
    req.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
    req.memory = V4L2_MEMORY_MMAP;
    if (ioctl(fd, VIDIOC_REQBUFS, &req) == -1) {
        cerr << "Error requesting buffers" << endl;
        close(fd);
        return 1;
    }

    v4l2_buffer buf;
    memset(&buf, 0, sizeof(buf));
    buf.type = V4L2_BUF_TYPE_VIDEO_CAPTURE;
    buf.memory = V4L2_MEMORY_MMAP;
    buf.index = 0;
    if (ioctl(fd, VIDIOC_QUERYBUF, &buf) == -1) {
        cerr << "Error querying buffer" << endl;
        close(fd);
        return 1;
    }

    void *buffer = mmap(NULL, buf.length, PROT_READ | PROT_WRITE, MAP_SHARED, fd, buf.m.offset);
    if (buffer == MAP_FAILED) {
        cerr << "Error mapping buffer to user space" << endl;
        close(fd);
        return 1;
    }

    // 开始捕获图像
    if (ioctl(fd, VIDIOC_STREAMON, &buf.type) == -1) {
        cerr << "Error starting capture" << endl;
        close(fd);
        return 1;
    }

    // 使用 OpenCV 显示图像
    Mat frame;
    namedWindow("Camera Feed", WINDOW_AUTOSIZE);
    while (true) {
        if (ioctl(fd, VIDIOC_QBUF, &buf) == -1) {
            cerr << "Error queuing buffer" << endl;
            break;
        }
        if (ioctl(fd, VIDIOC_DQBUF, &buf) == -1) {
            cerr << "Error dequeuing buffer" << endl;
            break;
        }
        frame = Mat(fmt.fmt.pix.height, fmt.fmt.pix.width, CV_8UC2, buffer);
        cvtColor(frame, frame, COLOR_YUV2BGR_YUYV); // 将 YUYV 格式转换为 BGR
        imshow("Camera Feed", frame);
        if (waitKey(30) == 27) // 按下 ESC 键退出
            break;
    }

    // 停止捕获图像
    if (ioctl(fd, VIDIOC_STREAMOFF, &buf.type) == -1) {
        cerr << "Error stopping capture" << endl;
    }

    // 释放资源
    munmap(buffer, buf.length);
    close(fd);
    destroyAllWindows();

    return 0;
}

